package ca.polymtl.rubikcube.util;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;

public final class Rotation {
	/**
	 * We use a quaternion to represent the rotation.
	 */
	private final Quaternion q;
	
	/**
	 * Creates a Rotation with default values (0 radians around the x axis).
	 */
	public Rotation() {
		this(0.0, new Vect3f(1, 0, 0));
	}

	/**
	 * Creates a Rotation from a rotation quaternion.
	 * 
	 * @param q
	 */
	public Rotation(Quaternion q) {
		this.q = q.normalized();
	}

	/**
	 * Creates a Rotation from an angle and a rotation axis.
	 * 
	 * @param angle
	 *            The angle in radians.
	 * @param axis
	 *            The normal rotation axis.
	 */
	public Rotation(double angle, Vect3f axis) {
		double scalar = Math.cos(angle / 2.0);
		Vect3f vector = axis.normalized().scale(Math.sin(angle / 2.0));

		q = new Quaternion(scalar, vector).normalized();
	}

	/**
	 * Adds the Rotation rot to this one (effectively applying 'this' then
	 * 'rot').
	 * 
	 * @param rot
	 */
	public Rotation add(Rotation rot) {
		//return new Rotation(rot.q.product(this.q));
		return new Rotation(this.q.product(rot.q));
	}

	/**
	 * Returns the rotation in form of a rotation matrix in a doubleBuffer
	 * (suitable to shoot to glMultMatrix).
	 * 
	 * @return
	 */
	public FloatBuffer asFloatBuffer() {
		ByteBuffer bb = ByteBuffer.allocateDirect(Float.SIZE / 8 * 16);
		bb.order(ByteOrder.nativeOrder());
		FloatBuffer buffer = bb.asFloatBuffer();

		Vect3f v = q.getVector();
		double w = q.getScalar();
		double x = v.x;
		double y = v.y;
		double z = v.z;
		double w2 = w * w;
		double x2 = x * x;
		double y2 = y * y;
		double z2 = z * z;

		float[] matrix = new float[16];

		matrix[0] = (float)(w2 + x2 - y2 - z2);
		matrix[1] = (float)(2 * x * y - 2 * w * z);
		matrix[2] = (float)(2 * x * z + 2 * w * y);
		matrix[3] = (float)(0);

		matrix[4] = (float)(2 * x * y + 2 * w * z);
		matrix[5] = (float)(w2 - x2 + y2 - z2);
		matrix[6] = (float)(2 * y * z - 2 * w * x);
		matrix[7] = (float)(0);

		matrix[8] = (float)(2 * x * z - 2 * w * y);
		matrix[9] = (float)(2 * y * z + 2 * w * x);
		matrix[10] = (float)(w2 - x2 - y2 + z2);
		matrix[11] = (float)(0);

		matrix[12] = (float)(0);
		matrix[13] = (float)(0);
		matrix[14] = (float)(0);
		matrix[15] = (float)(w2 + x2 + y2 + z2);

		buffer.put(matrix);
		buffer.position(0);

		return buffer;
	}
	
	/**
	 * Computes the rotation needed to go from 'other' to 'this'.
	 * 
	 * It can be thought as going back to the origin by applying the inverse
	 * of 'other', and then applying this rotation, thus "this * other^-1".
	 * @param other
	 * @return
	 */
	public Rotation difference(Rotation other) {
		return new Rotation(this.q.product(other.q.inverse()));
	}
	
	@Override
	public String toString() {
		return "Rotation [q=" + q + "]";
	}
}
